#!/usr/bin/python3
import logging
import os
import sys
import time

import numpy as np
import vtk
from IPython import embed
from path import Path
from PyQt5.QtCore import Qt
from PyQt5.QtCore import pyqtSignal as Signal
from PyQt5.QtCore import pyqtSlot as Slot
from PyQt5.QtGui import QGuiApplication, QIcon, QPainter, QPixmap
from PyQt5.QtWidgets import (QAction, QApplication, QComboBox, QDesktopWidget,
                             QFileDialog, QHBoxLayout, QHeaderView, QLabel,
                             QLineEdit, QMainWindow, QPushButton,
                             QSplashScreen, QSplitter, QTableWidget,
                             QTableWidgetItem, QVBoxLayout, QWidget)
from vtk.qt.QVTKRenderWindowInteractor import QVTKRenderWindowInteractor

from .Controller import Controller
from .VtkLoader import LoadMesh, LoadUrdf, RobotTree
from .ZenLogger import ZenLogger

ZenLogger(console_log_level=ZenLogger.INFO)


class MainWindow(QMainWindow):
    """
            RobotZen
    |----------------------------|
    |file|tools|                 |
    |----------------------------|
    | s |                        |
    | i |                        |
    | d |                        |
    | e |      canvas            |
    | b |                        |
    | a |                        |
    | r |                        |
    ------------------------------
    | status                     |
    ------------------------------

    TODO:
    Only GUI code should be kept here.

    Features:
    - add mesh visual node from obj/stl mesh file
    - scene graph tree
    - robot simulation support

    Next:
    - Fix robot FK/IK problem

    Plan:
    - select a visual node
    - pop up visual node property after selected
    - delete selected visual node
    - insert simple geometry visual node
    """
    LOGGER_COLORS = {logging.DEBUG: "grey", logging.INFO: "green",
                     logging.ERROR: "red", logging.WARNING: "yellow"}
    status_signal = Signal(str, int, name="status message")
    robot_target = Signal(np.ndarray, name="robot target joint positions")

    def __init__(self):
        super(MainWindow, self).__init__()
        self.setWindowTitle('RobotZen')
        self.setWindowIcon(QIcon('resources/robot.png'))
        size = QGuiApplication.primaryScreen().availableSize()
        self.setGeometry(0, 0, size.width(), size.height())

        # data
        self.controller = None
        self.iren = QVTKRenderWindowInteractor()
        self.renderer = vtk.vtkRenderer()
        self.iren.GetRenderWindow().AddRenderer(self.renderer)
        self.renderer.SetBackground(1.0, 1., 1.0)

        # Set initial camera position
        cam: vtk.vtkCamera = self.renderer.GetActiveCamera()
        cam.SetPosition(0, -5, 5)
        # cam.SetEyeTransformMatrix()

        # Set global coordinate axes
        axes0 = vtk.vtkAxesActor()
        axes0.SetPosition(0, 0, 0)
        axes0.AxisLabelsOff()
        axes0.DragableOff()
        axes0.SetShaftTypeToCylinder()
        axes0.SetTotalLength((0.4, 0.4, 0.4))
        self.renderer.AddActor(axes0)

        # self.iren.GetRenderWindow().AddRenderer(self.renderer)

        style = vtk.vtkInteractorStyleTrackballCamera()
        self.iren.SetInteractorStyle(style)

        self.iren.Initialize()
        self.iren.Start()

        # Menu
        self.menu = self.menuBar()
        file_menu = self.menu.addMenu("File")

        # Open file
        open_action = QAction("Open", self)
        open_action.setShortcut("Ctrl+O")
        open_action.triggered.connect(self.open_file)
        file_menu.addAction(open_action)

        # Exit QAction
        exit_action = QAction("Exit", self)
        exit_action.setShortcut("Ctrl+Q")
        exit_action.triggered.connect(self.exit_app)
        file_menu.addAction(exit_action)

        geom_menu = self.menu.addMenu("Geometries")
        sphere = QAction("Sphere", self)
        sphere.triggered.connect(lambda: print("Not implemented!"))
        sphere.triggered.connect(lambda: self.sidebar.update_tree())
        geom_menu.addAction(sphere)

        debug_menu = self.menu.addMenu("Debug")
        debug = QAction("Embed", self)
        debug.triggered.connect(embed)
        debug_menu.addAction(debug)

        # central zone
        splitter = QSplitter(Qt.Horizontal)

        # side bar
        self.sidebar = SideBar(
            "ModelTree", self.status_signal, self)
        splitter.addWidget(self.sidebar)
        splitter.addWidget(self.iren)
        splitter.setStretchFactor(0, 2)
        splitter.setStretchFactor(1, 8)
        self.setCentralWidget(splitter)

        self.status = self.statusBar()
        self.status_lbl = QLabel()
        # self.status.addPermanentWidget(self.status_lbl)
        self.status.addWidget(self.status_lbl)
        self.status_signal.connect(self.update_status)
        self.status_signal.emit('ready', logging.INFO)

        self.timer_id = self.startTimer(10)
        self.controller = None

    def exit_app(self, checked):
        QApplication.quit()

    def open_file(self):
        mesh_ext = set([".obj", ".stl", '.dae'])
        robot_ext = set([".urdf", ".rvdf"])
        points_ext = set([".txt", ".npy"])
        file = QFileDialog.getOpenFileName(caption="Select a file",
                                           directory=".", filter="Urdf(*.urdf);;Mesh(*.obj *.stl);;PointCloud(*.npy *.txt);;All(*)")[0]
        if not file:
            return
        if Path(file).ext in mesh_ext:
            LoadMesh(file, self.renderer)
            self.status_signal.emit(
                f"{file} has been loaded", logging.INFO)
        elif Path(file).ext in robot_ext:
            self.actors, self.robot_tree = LoadUrdf(self.renderer, file)
            self.status_signal.emit(
                f"{self.robot_tree.GetName()} has been loaded", logging.INFO)
            self.controller = Controller(self.actors, self.robot_tree)

        elif Path(file).ext in points_ext:
            try:
                pos = np.loadtxt(file)
                if pos.shape[1] == 3:
                    # TODO: Load point clouds
                    pass
                else:
                    self.status_signal.emit(
                        f"{file} can not be paresed as Nx3 array!", logging.ERROR)
            except Exception as e:
                self.status_signal.emit(
                    f"{file} can not be paresed as Nx3 array!", logging.ERROR)
        self.sidebar.update_tree()

    def timerEvent(self, ev):
        if self.controller:
            self.controller._Update()
        self.iren.Render()

    def update_status(self, msg, level, duration=10000):
        self.status_lbl.setStyleSheet(
            "color:{}".format(self.LOGGER_COLORS[level]))
        self.status_lbl.setText("{}".format((msg)))


class SideBar(QWidget):
    def __init__(self, desc_tree, status_signal, app, parent=None):
        super(SideBar, self).__init__(parent)
        vbox = QVBoxLayout()
        self.status_signal = status_signal
        self.app = app
        self.btn_scene_graph = QPushButton("SceneGraph")
        self.btn_scene_graph.clicked.connect(self.show_tree)
        vbox.addWidget(self.btn_scene_graph)

        self.btn_random = QPushButton("Random")
        self.btn_random.clicked.connect(
            lambda: self.app.controller.RandomState(False) if self.app.controller else None)
        vbox.addWidget(self.btn_random)

        self.btn_startserver = QPushButton("Start Controller Server")
        self.btn_startserver.clicked.connect(
            lambda: self.app.controller.StartServer() if self.app.controller else None)
        vbox.addWidget(self.btn_startserver)

        self.btn_stopserver = QPushButton("Stop Controller Server")
        self.btn_stopserver.clicked.connect(
            lambda: self.app.controller.StopServer() if self.app.controller else None)
        vbox.addWidget(self.btn_stopserver)

        self.tree_desc = QLabel(text=desc_tree, parent=self)
        vbox.addWidget(self.tree_desc)

        vbox.addStretch(1)

        self.btn_clear = QPushButton("clear")
        self.btn_clear.clicked.connect(self.clear)
        vbox.addWidget(self.btn_clear)
        self.setLayout(vbox)

    def show_tree(self):
        if self.tree_desc.isVisible():
            self.tree_desc.setVisible(False)
        else:
            self.update_tree()
            self.tree_desc.setVisible(True)

    def update_tree(self):
        # self.tree_desc.setText()
        pass

    def clear(self):
        self.update_tree()
        self.status_signal.emit("clear all visual nodes", logging.INFO)


class RobotControlPanel(QWidget):
    def __init__(self, parent=None):
        super(RobotControlPanel, self).__init__(parent)
        pass


def StartApp(*args, **kwargs):
    app = QApplication(*args)
    splash = QSplashScreen()
    splash.setPixmap(QPixmap('resources/robot.png'))
    splash.showMessage('Welcome to RobotZen',
                       Qt.AlignBottom | Qt.AlignCenter, Qt.blue)
    splash.show()
    time.sleep(2)
    window = MainWindow()
    window.show()
    splash.finish(window)
    sys.exit(app.exec_())


if __name__ == "__main__":
    StartApp(sys.args)
